//
// Created by benxb on 2022/3/6.
//


#include <ctime>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <LASlib/laswriter.hpp>
#include <timer.h>
#include <common/ini_reader.h>
#include <fstream>
#include <kitti_reader.h>
#include <or_reader.h>
#include <gtest/gtest.h>
#include <ns_reader.h>
#include <status_thread.h>
#include <toml.h>
#include <filesystem>

std::string out_dir_suffix = "/las/";

void Init(const std::string& out_dir) {
    // create data dir
    if (!std::filesystem::exists(out_dir)) {
        std::filesystem::create_directories(out_dir);
    }
}

void WritePoints(DataReader* data_reader, const std::string& data_dir) {
    if (data_reader == nullptr) {
        return;
    }
    // cpu and mem monitor
    benchmark::StatusThread statusThread(benchmark::GetCurrentPid(), "las", data_dir);
    std::thread stats_t(statusThread);

    // timer
    benchmark::Timer timer;
    timer.Start();
    std::string out_dir = data_dir + out_dir_suffix;
    Init(out_dir);

    // init header
    LASheader lasheader;
    lasheader.x_scale_factor = 0.000001;
    lasheader.y_scale_factor = 0.000001;
    lasheader.z_scale_factor = 0.000001;
    lasheader.x_offset = 0;
    lasheader.y_offset = 0;
    lasheader.z_offset = 0;
    lasheader.point_data_format = 1;
    lasheader.point_data_record_length = 28;

    // init point
    LASpoint laspoint;
    laspoint.init(&lasheader, lasheader.point_data_format, lasheader.point_data_record_length, nullptr);

    timer.StopAndRecord("Init time.");

    // open data file
    std::vector<float> x_arr;
    std::vector<float> y_arr;
    std::vector<float> z_arr;

    int sweep = 0;
    while(data_reader->HasMoreSweep()) {
        sweep++;
        if (sweep % 10 == 1) {
            std::cout << sweep << std::endl;
        }
        // read data
        data_reader->ReadSweep(x_arr, y_arr, z_arr);
        long long timestamp = data_reader->ReadTimestamp();

        timer.Start();
        // open laswriter
        LASwriteOpener lasWriteOpener;
        lasWriteOpener.set_file_name((out_dir + std::to_string(sweep) + ".las").c_str());
        LASwriter* pwriter = lasWriteOpener.open(&lasheader);
        if (pwriter == nullptr)
        {
            fprintf(stderr, "ERROR: could not open laswriter\n");
        }
        for (int i = 0; i < x_arr.size(); i++) {
            // populate the point
            laspoint.set_x(x_arr[i]);
            laspoint.set_y(y_arr[i]);
            laspoint.set_z(z_arr[i]);
            laspoint.set_gps_time(timestamp);

            // write the point
            pwriter->write_point(&laspoint);

            // add it to the inventory
            pwriter->update_inventory(&laspoint);
        }
        pwriter->update_header(&lasheader, TRUE);
        pwriter->close();
        delete pwriter;
        timer.Pause();
        x_arr.clear();
        y_arr.clear();
        z_arr.clear();
    }

    timer.StopAndRecord("Load time.");

    timer.Start();

    timer.StopAndRecord("Close time");
    timer.PrintAll();
    timer.SaveRecords(data_dir + "las_time.txt");
    benchmark::g_status_mtx.lock();
    stats_t.join();
    benchmark::g_status_mtx.unlock();
}

int main() {
    toml::table conf;
    try {
        conf = toml::parse_file("/home/chen/benxdb/benchmark/config.toml");
    } catch (const toml::parse_error& err) {
        std::cerr << "Parsing failed:\n" << err << "\n";
        return 1;
    }

    auto test_cases = conf["test_cases"].as_array();
    for (int i = 0; i < test_cases->size(); i++) {
        std::string case_name = test_cases->get(0)->value_or("");
        auto tcase = conf[case_name];
        auto reader_name = tcase["reader"].value_or("");
        auto data_dir = tcase["data_dir"].value_or("");
        DataReader *reader = DataReader::GetReader(reader_name, data_dir);
        WritePoints(reader, data_dir);
    }
}